/*
 *
 * Author: Andreas Wüstenberg (andreas.wuestenberg@rwth-aachen.de), Clemens Boennen (clemens.boennen@rwth-aachen.de)
 */
#include "Receiver.h"

#include "HelloWorld.h"

#include "ucdr/microcdr.h"

#define RECV_BUFFER_SIZE 128

Receiver_Init(struct Receiver *pRecv, uint32_t maxMsgSizeInBytes){
    pRecv->m_buffer = calloc(1, sizeof(uint8_t) * maxMsgSizeInBytes);
    Receiver_prepareRTPS(pRecv);
}

void Receiver_prepareRTPS(struct Receiver *pRecv){
    rtps_Domain_Construct(&pRecv->m_domain);
    rtps_Participant *part = rtps_Domain_createParticipant(&pRecv->m_domain);
    if(part == NULL){
        printf("Failed to create participant\n");
        return;
    }
    rtps_Domain_completeInit(&pRecv->m_domain);
    pRecv->mp_dataReader = rtps_Domain_createReader(&pRecv->m_domain, part, "HelloWorldTopic", 
                                            "HelloWorld", true, transformIP4ToU32(239,255,0,2));
    
    if(pRecv->mp_dataReader == NULL){
        printf("Failed to create endpoints.\n");
        return;
    }
    if (pRecv->mp_dataReader->m_bstateful)
        rtps_StatefulReader_registerCallback(pRecv->mp_dataReader, Receiver_responderCallback, pRecv);
    else
        rtps_StatelessReader_registerCallback(pRecv->mp_dataReader, Receiver_responderCallback, pRecv);
}
#if 0
static void Receiver_responderJumppad(void *callee, const struct rtps_ReaderCacheChange *cacheChange) {
    struct Receiver *receiverUnit = (struct Receiver *)callee;

    Receiver_responderCallback(receiverUnit, cacheChange);
}
#endif
void Receiver_responderCallback(void *callee, const ReaderCacheChange *cacheChange){
    struct Receiver *pRecv = (struct Receiver *)callee;

    bool success = rtps_ReaderCacheChange_copyInto(cacheChange, pRecv->m_buffer, RECV_BUFFER_SIZE);

    ucdrBuffer reader;
    ucdr_init_buffer(&reader, pRecv->m_buffer, RECV_BUFFER_SIZE);

    struct HelloWorld hw;

    uint8_t scheme[2];
    ucdr_deserialize_array_uint8_t(&reader, scheme, sizeof(SCHEME_CDR_LE));
    uint16_t options;
    ucdr_deserialize_uint16_t(&reader, &options);

    auto success2 = HelloWorld_deserialize_topic(&reader, &hw);

    if(success && success2){
        printf("Received Message %s with index %d\n", hw.message, hw.index);
    }else{
        printf("Received hello world message but copying or deserialization failed\n");
    }
}

void Receiver_run(){
    while(true); // Just serve all the time
}

